import java.io.*;
import lejos.nxt.*;
import lejos.nxt.comm.*;
import lejos.robotics.navigation.DifferentialPilot;
import java.lang.Math;

public class PRider implements Runnable {
    DifferentialPilot pilot;
    LightSensor sensor;
    int light, dark, basespeed = 40, speed = 40, rotate = 10;
    double error, kp, setpoint, value, result, bf;
    boolean running = true;
    
    public PRider() {
        pilot = new DifferentialPilot(56f, 110f, Motor.A, Motor.B);
        sensor = new LightSensor(SensorPort.S1);
    }
    
    public void go() {
        pilot.forward();
    }
    
    public void steer(double turnRate) {
        pilot.steer(turnRate);
    }
    
    public void setSpeed(double speed) {
        pilot.setTravelSpeed(speed);
    }
    
    public int readValue() {
        return this.sensor.readNormalizedValue();
    }
    
    public void calibrate() {
        light = this.readValue();
        dark = this.readValue();
        int value;
        
        pilot.setRotateSpeed(rotate);
        
        pilot.rotate(22.5, true);
        while (pilot.isMoving()) {
            value = this.readValue();
            if (value > light)
                light = value;
            if (value < dark)
                dark = value;
        }
        pilot.rotate(-45.0, true);
        while (pilot.isMoving()) {
            value = this.readValue();
            if (value > light)
                light = value;
            if (value < dark)
                dark = value;
        }
        pilot.rotate(22.5);
    }
    
    public void run() {
      calibrate();
      //double setpoint = (3 * l + d) / 4.0;
      setpoint = (light + dark) / 2.0;
      //double setpoint = 480;
      kp = 1.6;
      bf = .2;
      System.out.println("light: " + light + "\ndark: " + dark + "\nset: " + setpoint);        
      //Button.waitForAnyPress();  
      go();
      while (running) {
	value = readValue();
	error = setpoint - value;
	result = kp * error;
	speed = basespeed - (int) (Math.abs(error) * bf);
	setSpeed(speed);
	pilot.setRotateSpeed(rotate);
	steer(result);
      }
    }
    
    public static void main(String[] args) throws Exception {
      PRider linerider = new PRider();
      DataInputStream in;
      DataOutputStream out;
      Thread t = new Thread(linerider);
      NXTConnection connection = Bluetooth.waitForConnection();
      in = connection.openDataInputStream();
      out = connection.openDataOutputStream();
      t.start();
main: while (true) {
	int job = in.readInt();
	switch (job) {
	case(1):
	  out.writeInt(linerider.light);
	  out.writeInt(linerider.dark);
	  out.writeInt(linerider.basespeed);
	  out.writeInt(linerider.speed);
	  out.writeInt(linerider.rotate);
	  out.writeDouble(linerider.setpoint);
	  out.writeDouble(linerider.bf);
	  out.writeDouble(linerider.kp);
	  out.writeDouble(linerider.result);
	  out.flush();
	  break;
	case(2):
	  linerider.light = in.readInt();
	  linerider.dark = in.readInt();
	  linerider.basespeed = in.readInt();
	  linerider.speed = in.readInt();
	  linerider.rotate = in.readInt();
	  linerider.setpoint = in.readDouble();
	  linerider.bf = in.readDouble();
	  linerider.kp = in.readDouble();
	  linerider.result = in.readDouble();
	  break;
	case(3):
	  linerider.running = false;
	  break main;
	}
      }
    }
}